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@ A robot wtth controlled tool tracking displacement. 

(g) A tracking control robot controls Its arm end, which holds a 
tool, to track a taught path so that a reaction force between a 
workpiece and the tool remains constant. An Ideal state for the 
arm end is determined from the present state of the arm end 
and the difference between the actual reaction force and a 
predetermined target force. Then, this ideal state is compared 
with a reference state of the arm end on the taught path to 
obtain a target position and a target attitude of the arm end. 
Since a target position is atways determined independently of 
the last target position, no position error due to time delay 
affects a new target position even when the next target position 
is genefated before the tool reaches the last target position. 
Thus, the robot Is quick, responsive, and accurate when 
tracking at a constant operation speed. 
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Description 

A ROBOT WITH CONTROLLED TOOL TRACKING DISPUCEMENT 



BACKGROUND OF THE INVENTION 

5 

Field of the invention 

This invention relates to a tracking control robot for controlling the motion of a tool attached to the end of a 
robot arm so as to track a taught path while maintaining a desired force between the tool and a workpiece. 

10 Prior art 

For machining operations including burring, die polishing, trimming, and buffing, an industrial robot has 
been required to control a reaction force applied from an object to be machined (hereinafter a workpiece) and 
a tool, such as a grindstone attached to the end of a robot arm. 
One such robot was Introduced in "Force Control Syistem of a Light-weight Multl-artlculated Robot Arms": 
15 No. 3209 Report of the 3rd symposium of Robotics Society of Japan, held on November 28, 29. 30 In 1985. 
According to that report, the robot has, between Its arm end and a tool, a displacement/force conversion, 
mechanism with a predetermined relationship between a force applied to a predetermined axial direction and a 
displacement In the same axial direction. The robot also has a force sensor for detecting a reaction force 
applied by a workpiece to the tool. With this force sensor, the robot arm Is minutely controlled to adjust the 
20 detected reaction force to a predetermined value. 

The above-mentioned displacement/force conversion mechanism, which is also called a compliance 
mechanism, includes a resilient member such as a spring, a rubber section, etc. By adjusting the displacement 
In a predetermined axial direction, the force applied by the tool to the workpiece can be controlled. 
In this prior-art robot, the arm end is controlled to track a standard path determined by position data, which 
25 is stored during a preceding teaching process. While tracking the path, the force sensor Installed In the arm 
end detects the reaction force transmitted from the workpiece to the tool. 

By applying the detected reaction force to an equation of motion of a material particle, a position deviation A 
xi (the difference between the actual position of the arm end and a position of the arm end where a desired 
reaction force can be obtained) is calculated. Subsequently, a next target position xi for the arm end is 
30 calculated by adding the position deviation Axi to the last target position xl-1. Then, the arm Is moved to the 
calculated target position. 

By repeating this operation, the force applied from the toot to the workpiece is maintained at a constant 
value. 

As described above, the target position xi is calculated according to the following equation: 
35 xl = xI-1 + Axl 

Generally, however, the position where the reaction force is input from the force sensor is not the same as 
the last target position xi-1. This difference is caused because the correction signal for the position and 
attitude of the arm end is generated at intervals of several tens of milliseconds based on the reaction force 
detected by the force sensor while It actually takes several hundreds of milliseconds to move the arm end to 
40 the target position and attitude. As a result, another correction signal for the position and attitude of the arm Is 
generated before the arm attains the target position and attitude. 

This situation is shown In Figs. 14A and 14B, which Illustrate the movement of the ami end of the prior-art 
robot while finishing a workpiece having an uneven surface profile. The solid line In the drawings Indicates the 
actual movement of the robot arm end, and the broken line Indicates the target value of the movement of the 
45 arm end, and the chain line indicates the ideal position of the arm end with a predetermined reaction force. The 
arm end travels from the left to the right in the drawing while machining the workpiece. 

In Fig. 14A, the displacement of the compliance mechanism, which corresponds to the output of the force 
sensor, Is used as the position deviation Ax without any modification. In this case, the arm end travels as 
follows. 

50 Until the arm end reaches the point N »2, the distance between the arm end and the workpiece is constant, 
so that the reaction force of the workpiece against the tool is also constant. 

After the arm end reaches the point N «2, the distance between the arm end and the workpiece exceeds a 
predetermined value, and the force applied from the tool to the workpiece decreases. To compensate for the 
loss, the target value of the next point (N «»3) is set by adding the difference between the ideal position and the 
55 actual position of the arm end to the target value of the last point (N=2). 

At the time point N « 3, the distance between the arm end and the workpiece still exceeds a preset value, so 
that the target value of the next point (N=4) must be corrected. At the point N= 3, however, the arm end has 
not yet reached the target point. Accordingly, the non-adjusted position deviation is added to the target value 
of the next point (N »4). The non-adjusted position deviations are successively accumulated in the following 
60 points. As a result, the control system causes overshooting and oscillation as shown in Fig. 14A. 

To solve the above-mentioned problems, it is possible to use a position deviation that is determined by 
multiplying a coefficient smaller than one (e.g., 1 /3 or 1/4) and the displacement of the compliance 
meclianism, which corresponds to the force sensor output. 
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In this case, however. It takes longer to achieve a desired value for the force applied from the tool to the 
workplece, even though overshoot never occurs. In Fig. 14B, for example, It takes ten process steps. 

VSfhiie the force applied by the tool to the workplece is outside the desired value, machining accuracy 
deteriorates. 

In order to solve the problem caused by the time lag between the time when the target value Is output and 5 
when the arm actually attains the target value, it Is possible to output the next correction signal after the arm 
attains the target value. 

In this case, since It takes some hundreds of milliseconds after the output of the target value to bring the 
arm into the target position, as described above, each control step also requires some hundreds of 
miliiseconds. Accordingly, the machining speed is considerably decreased. This method, therefore, Is not 10 
practical. 

The above-mentioned conventional robot traces the standard path 'based on the taught data. If some 
con-ection is required for the position of the robot arm, the arm may markedly deviate from the standard path 
because the position Is corrected independently of the taught data. 

IS 

SUMMARY OF THE INVENTION 

An object of the present invention is to solve the above-mentioned problems and to provide a tracking 
control robot that holds teaching data for determining an accurate target position such that a desired reaction 
force applied by a workplece to a tool can be obtained. 

Another object of the present invention is to provide a tracking control robot that adjusts the reaction force 20 
applied by the workplece to the tool In real time without reducing the robot operation speed. 

A further object of the present Invention is to provide a tracking control robot with improved responsiveness 
and accuracy for controlling the position and attitude of the tool. 

To achieve these and other objects, the present invention has the following constitution. As shown In Fig. 1, 
the tracking control robot of the present invention has a tool R2 at an arm end R1 of the robot via a 25 
displacement-to-force conversion mechanism R3 that holds a predetennlned relationship In a particular 
coordinate system between the force applied to one axial direction and the displacement In the same axial 
direction. The robot controls the arm end R1 to move the tool R2 to track a standard path obtained from 
prestored position data and to act on a workplace W with a preset target force. The robot further comprises: 
present-condition detection means R4 for detecting a present position and attitude of the arm end R1; 30 
reaction-force detection means R5 for detecting a reaction force applied by the workplece W to the tool R2 at 
the detecting point of the present position and attitude of the ami end R1 ; displacement conversion means R6 
for calculating a difference between the detected reaction force and the preset target force of the tool R2 at 
the detecting point of the present position and attitude of the ann end R1, and for converting the difference 
Into the displacement In the position of the tool R2; ideal-condition calculation means R7 for calculating an 35 
ideal position and attitude of the arm end R1 based on the displacement of the tool R2 and on the present 
position and attitude of the arm end R1 ; correction-amount calculation means RB for calculating a correction 
amount for the position and attitude of the arm end R1 by comparing the ideal position and attitude of the arm 
end R1 with a reference position and attitude of the arm end R1 on the standard path; and arm-end control 
means R9 for controlling the position and attitude of the arm end R1 in real time according to the correction 40 
amount for the position and attitude of the arm end R1. 

BRIEF DESCRIPTION OF THE DRAWINGS 

Fig. 1 is a block diagram showing a fundamental stmcture of the present Invention. 45 
Fig. 2 Is a perspective view of a robot and a control system as an embodiment of the present Invention. 
Figs. 3A. 3B. and 3C are a plan view, a front view, and a left side view, respectively, of an operational 
body of the robot. 
Fig. 4 Is a perspective view showing a robot arm end. 

Fig. 5 Is a block diagram showing the constitution of a robot controller. SO 

Fig. 6 Is a block diagram showing the control section of the robot controller. 

Fig. 7 is an illustration indicating positional relationships among different control points. 

Fig. 8 Is an illustration of force data detected by a force sensor. 

Fig. 9 is an illustration showing the detection of a change In the reaction force. 

Fig. 10 is a flow chart indicating control processes executed in a first CPU. 55 

Fig. 1 1 is a flow chart indicating control processes executed In a second CPU. 

Fig. 1 2 Is an Illustration showing the movement of the robot arm end of the present embodiment. 

Fig. 13 Is a perspective view showing another example of a compliance mechanism. 

Figs. 14A and B are illustrations of the movement of the arm end of prior-art robots. 

60 

DETAILED DESCHIPTION OF PREFERRED EMBODIMENT 

The present embodiment relates to a 6-axis articulated robot for grinding or polishing a workplece by using 
a grindstone as a tool, while keeping a constant reaction force between the workpiece and the tool. 

The construction of the robot 10 is shown in Fig. 2. The robot 10 is equipped with a robot controller 12 for 
controlling the operation of the robot 10; a teaching box 14 on the robot controller 12 that enables an operator 65 
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to teach the robot 10 In advance how to do a task; an operational body 16 with six axes that Is actuated in 
accordance with signals sent from the robot controller 12; a tool 20, and a force sensor 22 for detecting 
reaction force of a workpfece W against the toot 20. both being installed on a compliance mechanism 19 at an 
arm end 1 8 on the operational body 16; and a sensor controller 24 for processing the signal sent from the force 
5 sensor 22. Moreover, the robot controller 1 2 includes a temiinal T for Input and output, and a printer P. 

. Detail explanations for each part of the robot 1 0 are set forth. First, the operational body 1 6 of the robot 1 0 Is 
explained with reference to Figs. 3A, 3B, and 3C, respectively showing a plan view, a front view, and a left side 
view of the operational body 16. The operational body 16 has six axes of rotation. The first, second, and third 
axes control the position of the arm end 18 in relation to the workpiece W; and the fourth, fifth, and sixth axes 

10 control the attitude of the arm end 18 In relation to the workpiece W. 

Each axis has a motor, a brake for stopping the rotation, and an encoder for detecting the rotation speed of 
the axis: the first axis includes a first motor 30, a first brake 32, and a first encoder 34; the second axis includes 
a second motor 40, a second brake 42, and a second encoder 44; the third axis Includes a third motor 50, a 
third brake 52, and a third encoder 54; the fourth axis has a fourth motor 60, a fourth brake 62, and a fourth 

IS encoder 64; the fifth axis includes a fifth motor 70, a fifth brake 72, and a fifth encoder 74; and the sixth axis 
Includes a sixth motor 80, a sixth brake 82, and a sixth encoder 84. 

Rotations about the second axis and the third axis by the motors 40 and 50 are converted into linear 
movements via bail-screw feeds 90 and 92, and the linear movements are converted Into rotations about the 
axes by means of parallel links. Rotations about the other axes are actuated by the respective motors via 

20 torque tubes, reduction gears, helical gears, and other transmitting members. 

in Fig. 4, the arm end 18 has a grindstone 20. a motor 100, a compliance mechanism 19, and a force sensor 
22. The grindstone 20 is a tool for machining the workpiece W, The motor 100 is connected to the grindstone 
20 via a gear ( not shown ) so as to rotate the grindstone 20. The compliance mechanism 19 is composed of a 
spring 102 and dampers 104, and slides with reaction forces only In the displacement directions shown in 

25 Fig. 4. The force sensor 22 is Installed between the compliance mechanism 19 and the arm end 1 8 to detect the 
reaction force applied by the workpiece W to the grindstone 20. The relationship between the displacement of 
the compliance mechanism 19 and the force Is determined in advance. 

The force sensor 22 in this embodiment uses a strain gauge for detecting forces in three axes, and torques 
around the three axes, consisting of an axis along the displacement of the compliance mechanism 19 and two 

30 axes that are perpendicular to the displacement axis and also perpendicular to each other. 

Force data Fsen(KH1) detected by the force sensor 22 represents the force and the moment along the each 
axis in a sensor coordinate system 0-Xs-Ys-Zs with the origin 0 at the position of the force sensor 22. The force 
data is expressed by the following six-dimensional vector. 
Fsen « (Fxs, Fys, Fzs, MUxs, MNys, MNzs^, where 

35 T indicates a transposed matrix, F means force, MN means moment, and xs, ys, and zs are the sensor axes. 
In this embodiment, the zs-axls of the sensor coordinate system corresponds to the displacement axis of 
the compliance mechanism 19. 

Set forth next is the constitution of the robot controller 12 that controls the 6-axls operational body 16 and 
also acts as a teaching system for the robot 10. 

40 The robot controller 12. diagramed in Fig. 5, Is a microcomputer consisting of a first CPU 300, a second CPU 
301 , a ROM 302, a RAM 304, and a backup RAM 306, which are well-known and are connected to input/output 
ports via a bus 308. 

Input/output ports, which cooperate with the first CPU 300 and the second CPU 301 as major components 
of the robot controller 12, include: a bubble-memory interface 312 for a bubble memory 310; a temiinal 

45 input/output port 314 for a terminal T having a CRT display and a keyboard; a printer output port 316 for a 
printer P; a teaching box input port 318 for the teaching box 14; a sensor Input port 230 for receiving electrical 
signals sent from the force sensor 22 via a sensor controller 24, and for generating an Interrupt in the second 
CPU 301 ; and servo circuits 320, 322, 324, 326, 328, and 330 for the first through sixth axes, respectively. 
Each servo circuit 320 through 330 is of the Intelligent type with its own CPU. When the target position data 

SO and the target attitude data for the grindstone 20 are sent from the first CPU 300 via the bus 308, the control 
amount for individual six axes are input from the encoders 34. 44, 54. 64, 74, and 84. Based on these control 
amounts, the first through sixth motors 30, 40, 50, 60. 70, and 80 are driven via the respective drivers 332. 334, 
336, 338, 340, and 342. Then, the tip of the grindstone 20 is brought into the target position and attitude. 
Moreover, the first through sixth servo circuits 320, 322, 324, 326. 328, and 330 respectively drive the first 

S5 through sixth brakes 32, 42. 52, 62, 72. and 82 as the occasion demands so as to prevent the arm from 
operating or falling down from 1 1 s dead weight when the power supply for the servo circuits 320 through 330, 
or for the motors 30, 40, 50, 60, 70, and 80 is turned off for an emergency stop. 

The operation of the robot controller 12 Is hereinafter described. The control system of the robot controller 
12 consists mainly of two sections. I.e.. the first and second CPUs 300 and 301, as shown in Fig. 6. The first 

60 CPU 300 sends standard-path data PTpath(KHI), which is obtained by Interpolating the previously taught data, 
to the second CPU 301. Then, the first CPU 300 receives an initial target value PTmod(KHI) modified by a 
correction amount. Based on this initial target value PTmod(KHI), a final target value host(KHI) for each axis is 
calculated and sent to each servo circuit, which Is the operation control section. 
On the other hand, the second CPU 301 calculates a correction amount APT(KHI) based on the force data 

65 from the force sensor 22, the attitude data of the arm end 18 concurrent with the force data, and the 
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standard-path data PTpath(KHI) from the first CPU 300. By adding an offset amount to the calculated 
correction amount, the second CPU 301 calculates a target value PTmod(KHI) to send It to the first CPU 300. 

Set forth is a detailed explanation of the control by the first CPU 300. Before starting a particular job with the 
robot 10, an operator uses the teaching box 14 to teach the robot 10 which work points on the workplece W 
the tool should trace and what attitude the tool should take, in the same manner as with conventional robots. 
Such data Is stored In a teaching-point memory 400 In the RAM 304. 

The teaching-point data represents the values linking the base coordinates of the robot and the work point 
of the tool. The m-th point of the teaching data is shown by the following matrix: 

PTtch[m] =rNX OX AX X^ 10 

NY OY AY Y 



NZ OZ AZ Z 
0 0 0 1 



. . . (1 ) , wherein 



15 



wherein 

NX, NY, and NZ are the X-Y-Z components of a normal vector; OX. OY, and OZ are the X-Y-Z components of 
an orientation vector; AX. AY. and AZ are the X-Y-Z components of an approach vector; and X. Y, and Z are the 
X-Y-Z components of a position vector of the m-th teaching point. Every component of the vectors is 
represented by a coordinate system 0-XYZ (see Fig. 2) with the origin 0 at the predetermined standard point of 
the operational body 16. 20 

The normal vector, the orientation vector, and the approach vector, perpendicular to one another, indicate 
the working direction of the tool at the teaching point shown by the above matrix. The variables for indicating 
the points referred to in the following explanation are also represented by the same type of matrix. 

After the teaching operation, the robot 10 starts the particular Job In response to an operation start signal. 
First of all, a path Interpolating circuit 402 interpolates KHO-th teaching-point data PTtch(KHO) sent from the 25 
teaching-point memory 400. and then calculates a standard-path data PTpath(KHI) for the tool 20. This 
standard-path data PTpath(KHI) defines a standard path for the tool 20 to follow. Subsequently, the 
standard-path data PTpath(KHI) Is sent to the second CPU 301, which produces modlfied-target-point data 
PTmod(KHI) that the CPU 301 sends to an Inverse kinematic transformation circuit 404. 

The inverse kinematic transformation circuit 404 transforms the modified-target-point data PTmod(KH1 ) into 30 
rotation angle data 8host(KH1 ) for each of the six axes. The rotation angle data are sent to the con^esponding 
servo circuits 320 through 330. Then, the motors 30. 40. 50. 60, 70, and 80 for each axis are driven via the 
corresponding driver circuits 332 through 342 so as to shift the work point of the grindstone 20 to the point 
corresponding to the modified-target-point data PTmod(KHI). Fig. 7 shows the relationships among the 
grindstone 20, the workplace W. PTpath(KHI). APT(KH1), and PTmod(KHI). 35 

The following is the explanation of the second CPU 301 for determining modified-target-point data 
PTmod(KHI), referring to Fig. 6. The second CPU 301 executes its process in parallel with the first CPU 300. 
The time interval in the first CPU 300 for transmitting PTpath(KHI) to the second CPU 301 is equal to the total 
time interval in the second CPU 301 for receiving PTpath(KHI) from the first CPU 300, calculating the 
con-ection data PTmod(KHI) and transmitting it back to the first CPU 300. 40 

On receiving PTpath(KHI) from the first CPU 300. the second CPU 301 starts its control process. First, each 
set of rotation angle data eenc(KH1 ) from the encoders 34. 44, 54, 64. 74, and 84 of the six axes is transformed 
Into flange-end data PF(KH1 ) in the kinematic transformation circuit 406 by using the following equation. 
PF(KHI) = A[8enc(KH1)] (2) 

where Ms a kinematic transfonnatlon. and [8enc(KH1)] is an angle vector. 45 

Subsequently, a tool-work-point-coordinate/sensor-coordinate calculation circuit 408 calculates sensor- 
end data PS(KH1) and actual position data PTenc(KHI) by using the flange-end data PF(KHI) In the following 
formulas (3) and (4). 
PS(KH1) = PF(KHI) 'Tif^s (3) 

PTenc(KHI) = PF(KH1).7if^ (4). In which SO 
nt-^ Is a position/attitude matrix representing the position of the sensor end In relation to the flange end of the 
tool, and Tif-^t is a position/attitude matrix representing the position of the work point in relation to the flange 
end. 

The purpose of the above conversion is to convert the force and the moment of each axis at the force 
sensor 22, into those at the position of the tool 20. 55 

When the flange-end data PF(KHI) is input, force data Fsen(KHI) Is also input from the force sensor 22 via 
the sensor controller 24. As explained before, the force data Fsen(KHI) represents force and moment of each 
axis of the sensor coordinate 0-Xs-Ys-Zs with the origin 0 at the position of the force sensor 22. The force data 
Fsen(KHI) is represented by a 6-dimensional vector. 

The force data Fsen(KH1) is composed of a weight Mtool of the tool 20 and a reaction force Fw applied from 50 
the workpiece W to the tool 20. In this embodiment, it is supposed that the reaction force Fw Is equal to a 
target value Fzo which is stored In advance in a target-force memory 412 of the RAM 304. As the robot 10 
moves in free space and changes the attitude of the anm end 18, the tool weight for each of the six axes 
detected by the force sensor 22 also varies. 

To facilitate the explanation of the tool weight change from the tool attitude change, the tool 20 is adjusted 65 
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only in the sliding direction of the compliance mechanism 19, l.e., the displacement direction. Moreover, the 
sliding direction corresponds to both the Zs-axis of the sensor coordinate system and the displacement 
direction of the compliance mechanism 19 shown in Fig. 4. As shown in Fig. 8, a Zs-axis component of the 
force data Fsen is a difference between a Zs-axis component Ftool of the tool weight and the reaction force Fw 

5 Of the workpiece W. When the angie between the correction direction (Zs-axis) and the gravitational direction 
(chain line) is 6, the Zs-axis component Ftool of the tool weight is calculated by a formula Ftool = Mtool -cosG. 
If the reaction force applied from the workpiece W to the tool 20 Is represented by Fw, the Zs-axis component 
Fzs of the force data Fsen is calculated according to the following formula: 
Fzs = Mtool-cosB - Fw 

10 = Ftool -Fw (5) 

The tool weight applied to each axis Is calculated in the tool weight calculation circuit 410 in Fig. 6. First, the 
value cos6 Is calculated based on the sensor coordinate data PS(KHI). Next, the tool weight Ftool In the 
con-ectional direction (Zs-axIs) is calculated by using the prestored tool data Mtool. 

If the X-Y plane of the robot coordinate system is parallel to the ground (see Fig. 2), the Z-axis direction of 
15 the robot coordinate system 0-X-Y-Z is identical with the gravitational direction, so that the value of cos9 can 
be obtained by extracting a component of the sensor coordinate data PS(KH1). Namely, the sensor coordinate 
data PS(KH1) represents a relative position and attitude of the sensor coordinate system In relation to the 
robot coordinate system 0-X-Y-Z. It Is shown as follows: 



20 



25 



PS(KHI) = 



^Xsx 


Ysx 


Zsx 


Xi 


Xsy 


Ysy 


Zsy 


Y 


Xsz 


Ysz 


Zsz 


Z 


Iq 


0 


0. 


1 J 



... (6) 



Accordingly, the angle cos9 defined by the Zs-axis of the sensor coordinate system and the Z-axis of the robot 
coordinate system is shown as follows: 
cos9 = Zsz (7) 

On the other hand* if the X-Y plane of the robot coordinate system Is not parallel to the ground, the angle 
30 cos9 is an Inner product of a vector G in the gravitational direction of the robot coordinate system, i.e., (X, Y, 
Z) ^ (0, 0. 1), and the Zs-vector (Zsx. Zsy, Zsz) of the sensor coordinate data PS(KHI). 
cose = (G.Zs)/||G||-||Zs|l (8) 

If a reaction force greater than a target force Fzo by AF is applied, such a difference AF can be specified in 
the following manner, in Fig. 9, dFtool, dFw, and dAF represent the displacement of the compliance 
35 mechanism 19 corresponding to the forces Ftool, Fw and AF, respectively. The arm end 18 shown in the left 
side of the drawing is In a state In which the reaction force applied from the workpiece W is equal to the target 
force Fzo. On the other hand, the ami end 18 shown in the right side of the drawing is In a state In which the 
arm end 18 has l3een moved to the right portion of the workpiece W where the distance between the workpiece 
W and the arm end 18 is shorter by dO while the reaction force Fw from the workpiece W Is greater by AF. 
40 ' When the reaction force Fw of the workpiece W is equal to the target force Fzo, the relationships shown by 
the formula (5) is satisfied, if the force AF is added to Fw, a relationship sham by the following formula (9) Is 
satisfied. 

Fzs =i Ftool - (Fw + AF) (9) 

Since the values Fzs, Ftool, and Fw are already known, the additional force AF is obtained by using the 
45 following formula: 

AF = -Fzs + Ftool - Fw (10) 

The calculated AF is the amount needed to correct the reaction force. The position deviation Ad needed to 
make AF zero Is obtained in a deviation conversion circuit 414 in Fig. 6. 
If the tool weight Mtool (hereinafter referred to as M) below the sensor Is a material particle, a relation shown 
50 in the following equation of motion results. 
AF = Mi5 -I- CD + KAd (11) 

In the above formula. D represents the tool position in the direction of Zs-axIs based on the position of the 
sensor, c and K respectively Indicate a viscosity constant and a spring constant of the compliance mechanism 
19. From the formula (11), the position deviation d Is obtained as follows: 
55 Ad « (AF - Mb - CD) / K (12) 

Because the above calculations are executed every discrete cycle, Ad(KHI) is approximated as follows. In 
the following equation, x is one cycle time. Ad(KHI-l) Is the position deviation Ad during the last cycle, and 
Ad(KiH1-2) is Ad during the second-to-last cycle. By approximating b and D using Ad(KHI-l) and Ad(KIH1-2), 
Ad(KHI) is approximated as: 
60 Ad(KH1) « 1/K.[AF 

- M/t2 {D(KH1-2) - 2D(KH1 - 1) + D(KH1)} 

- c/T {D(KHI-I) - D(KH1)}] (13), where 

65 
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D(KHl) = 



ri 0 0 0 

0 10 0 . 
0 0 1 Jd(KHI) 



I 



If M and c 0 In the above-mentioned formula, then Ad(KHI) = AF/K. As a result, the Inertia and viscosity 
terms would be delayed one cycle. However, since Ad(KH1) fc? Ad(KH1 - 1) when one cycle t is shorter than 
one hundred milliseconds. It Is possible to calculate the position deviation Ad(KH1) In relation to AF Ignoring 

the negligible cycle delay. 

In some cases, such as when control Is a static, or when M and the constant c are extremely small values, 
both M and c can be eliminated from this calculation. 

In the present embodiment, the position deviations Ad(KHM) and Ad(KH1-2) (i.e., the values one cycle and 
two cycles prior to Ad(KHI)) are used for approximating the tool position D. It Is also possible to use Ad(KH1 
+ 1) and Ad(KH1 +2) (I.e., the values one and two cycles after Ad(KHI)) to approximate the tool position, as 15 
shown in the following fomnula. 
Ad(KHI) = 1/K.[AF 

-M/t2{d(KHl)-2D(KH1 + 1) + D(KH1 + 2)} 
- c/T {D(KH1 + 1) - D(KH1 + 2)}] (13') 

Based on the position deviation amount Ad(KH1 ) In relation to the force AF and the sensor coordinate data 20 
PS(KH1). an ideal position PSalm(KHI) of the sensor 22. at which the reaction force Is equal to the target force 
Fzo(KH1), is calculated in an ideal position calculation circuit 416 (Fig. 6) using the following formula: 
PSalm(KHI) = PS(KH1).D(KH1) (14) 

n*-^ Is calculated as follows by using PTenc(KHI) calculated at step 408. 
Tta^t « PSaim(KH1)-< .PTalm(KHI) (14)' 25 
PTalm(KHI) - PSalm(KH1)-Ka-.t (15). where 

7U-*t represents a matrix Indicating the position and attitude of the tool-work-point coordinates in relation to 
the sensor coordinates. 

The Ideal position PSaim(KHI), which Is data In the sensor coordinate system. Is converted into data 
PTaim(KHI) In the robot coordinate system in a tool work-point conversion circuit 418 (Fig. 6) in accordance 30 
with the following formula: 

Subsequently, equation (16) Is used to obtain a deviation R-ofs(KHI) between the ideal position PTalm(KHI) 
and the standard-path data PTpath(KHI) that has been obtained using the path Interpolation circuit 402. 
R-ofs(KHI) = PTpath(KH1)-'< .PTalm(KHI) (16) 

The ratio R-ofs(KHI) is used to correct the position in the conrectlon direction and to maintain a taught 35 
speed In the proceeding direction of the tool, which Is normally perpendicular to the correction direction. This 
correction using R-ofs(KHI) is necessary because the actual tool position is, in effect, behind the 
standard-path position PTpath(KHI) as shown in Fig. 7, and the ideal position of the tool work point 
PTaim(KHI) corresponding to the actual tool position Is also behind. Correction with those delayed values will 
change the moving speed of the tool. ^ 

Subsequently, a correction-element-selection circuit 420 (Fig. 6) detenmines a con-ection amount APT(KH1 ) 
by applying the ratio R-ofs{KH1) only to the matrix component that corresponds to the element detected by 
the force sensor 22. The correction amount APT(KHI) Is determined as follows: 



/fPT(KH1 ) 



1 0 0 0^ 45 

0 10 0 

0 0 1 Z 

LO 0 0 lJ ... (17) 

In the above matrix, Z is the same as the element Z in the ratio R-ofs(KH1 ). The conrectlon amount APT(KH1 ) is 50 
sent to an abnormality detection circuit 422 and compared with a predetermined value p. If the absolute value 
I |APT{KH1)1 1 of APT(KHI) Is greater than p, the robot may be running off the desired path. In that case, 
emergency action, such as emergency stop, is taken. On the other hand, if 1 1 APT(KH1) 1 1 Is smaller than p, 
next work-point correction data PTpmod(KHI) is calculated using the correction amount APT(KHI) according 
to the following formula. ^ 
PTpmod(KHI) » PTpath(KH1).APT(KH1) (18) 

Subsequently, modifled-target-point data PTmod(KHI) Is detenmlned based on an offset Ofs(KHI) from the 
target point in a target offset circuit 424, according to the following formula. 
PTmod(KHI) « PTpmod(KH1)-Ofs(KH1) (19), where 

PTpmod(KHI) indicates an initial target tool-end coordinate. ^ 
The modified-target-point data PTmod(KHI) is transmitted to the first CPU 300, and the process In the 

second CPU 301 is completed. 
The above-mentioned processes executed in the first CPU 300 and the second CPU 301 are shown by 

flowcharts of Figs. 10 and 11. respectively. The robot controller 12 of the present embodiment includes two 

CPUs 300 and 301 shown in Fig. 6. These two CPUs 300 and 301 carry out their respective procedures In 65 
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parallel with each other. 

When the robot 10 starts a particular Job In response to the operation start signal, the first CPU 300 
proceeds through the following steps in sequence: 

Step 500: Calculates the standard-path data PTpath(KHI) by interpolating the teaching-point data PTtch(KHI) 
5 stored In the memory 400. 

Step 502: Send the standard-path data PTpath(KHI) to the second CPU 301. and request the 

modifled-target-point data PTmod(KHI) from the second CPU 301. 

Step 504: Receive PTmod(KHI) from the second CPU 301. 

Step 506: Convert PTmod(KHI) into the rotation angle ehost(KHI) for each axis. 
10 Step 508: Send the rotation angle ehost{KH1) to the servo circuits 320 through 330 for the six axes. 

Step 510: Increment KH1 by 1, and returning to step 500 to repeat the steps 500 through 510. 
The second CPU 301 executes the following steps: 

Step 600: Receive the standard-path data PTpath(KHI) from the first CPU 300. 

Step 602: Receive the force data Fsen(KHI) from the sensor controller 24. 
15 Step 604: Receive the rotation angle data 6enc(KH1 ) from the individual axis encoders 34. 44, 54. 64, 74, and 64 

at the moment when the force sensor 22 starts its measurement. 

Steps 606, 608. 610. 612. 614, 616, and 618: Calculate Ftool. AF, Ad(KHI). PSalm(KHI), PTaim(KHI). 
R-ofs(KHI). and APT(KHI) according to their corresponding formulas. 

Step 620: Compare the correction amount APT(KHI) with the predetermined value p. If 1 1 APT(KHI) 1 1 ^ P, 
20 the program proceeds to step 628. If I |APT(KH1)| I < p, it proceeds to step 622. 
Step 622: Calculate the work-point correction data PTpmod(KHl), 
Step 624: Calculate the modified-target-point data PTmod{KH1). 

Step 626: Send the modlfied-target-poInt data PTmod(KHI) to the first CPU 300, and return to step 600. 
Step 628: Stop the robot 10 as an emergency measure, and terminate the process. 
25 By repeating the above-mentioned process steps, . the robot 10 continues operation while adjusting the 
force of the grindstone 20 against the workplace W according to the target force. 

Fig. 12 illustrates the movement of the arm end 18 when a workplace W identical to the ones used for the 
conventional robots In Figs. 14A and 14B is machined by the robot of the present embodiment. In Fig. 12. the 
solid line indicates the movement of the ami end 18. the broken line indicates the target value of the arm end 
30 18, and the chain line indicates the ideal position of the arm end 18 with the ideal reaction force. While 
machining the workpiece, the robot arm end travels from left to right in the drawing. Fig. 12 clearly shows that 
the robot of the present embodiment, in contrast to the prior-art robot, can quickly track the contour of the 
workpiece W without overshooting. 
The present embodiment provides the following effects. 
35 (1) Since the correction amount for determining the target point is calculated based on the Information 

sent from the force sensor 22 and the information concerning the position and attitude of the grindstone 
20 on the sensing point, the robot can carry out a particular job quite accurately without reducing its 
operation speed. 

(2) Since the target point is determined based not only on the information from the force sensor 22 but 
40 also on the teaching points along a reference path, the robot never deviates from the desired path. 

(3) Target points are successively determined in a cycle as short as less than one hundred 
milliseconds, so that precise control can be realized. 

(4) After the target value is calculated, the position and attitude of the tool 20 are controlled based on 
the.servo circuits of the sfoc axes In the same manner as in the prior art. The robot, therefore, can be used 

45 as an ordinary teaching play-back robot If force control is not required. 

(5) Since the actual control of the position and attitude of the tool 20 Is same as that of the prior-art 
robots, the present embodiment can be realized by supplying only a new CPU board and a new interface 
for the prior-art teaching play-back robot controller. Consequently, it can inexpensively produce the 
force-controllable robot. 

50 (6) Besides being a machining robot, this robot can also be used as a balancer: when the operator 

holds the tip of the force sensor 22 by hand to easily transport a heavy material supported by the robot, 
the balancer controls the transporting direction. 

(7) When the correction amount APT is calculated, an element of the matrix is appropriately provided 
for each calculation, so that the force is only adjusted in a required axial direction. For an axis with no 

55 need of force control, only the position control Is provided. 

(8) Since the robot adjusts the dead weight of the tool, It can carry out a particular job in an optional 
attitude independent of gravity. 

While the Invention has been shown and described with reference to one preferred embodiment, it should 
be understood by those skilled In the art that various other changes In form and detail may be made without 
60 departing from the spirit and scope of the invention. 

For example, the present embodiment uses a compliance mechanism 19 (see Fig. 4) that slides 
perpendicularly to the axis of the grindstone 20. it is also possible to use a compliance mechanism 19a that 
slides In parallel to the axis of a grindstone 20a as shown by an arrow in Fig. 13. For these compliance 
mechanisms 19 and 19a, a remote center compliance system, which is generally called a RCC, can be used. 
65 In the present embodiment, the tool position data from the encoders 34, 44, 54, 64, 74, and 84 are converted 
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from the robot coordinate system to the sensor coordinate system In the coordinate conversion circuit 408. 
The Ideal position is then calculated in the form of the sensor coordinate system, and converted to the robot 
coordinate system again In the coordinate conversion circuit 418. Alternatively, It Is also possible to convert 
the force data from the sensor coordinate system Into the robot coordinate system so as to be used In the 
calculation of the ideal position. In such case, the coordinate conversion circuits 408 and 418 In Fig. 6 are 5 
omitted, and another coordinate conversion circuit is provided after the sensor controller 24 in order to 
convert the force data from the sensor coordinate system to the robot coordinate system. In this case, too, the 
calculations described in the present embodiment can be used, though each value would be indicated In the 
robot coordinate system. 



Claims 

1. A tracking control robot having a tool at an end of an arm of the robot with a displacement-to-force 
conversion mechanism that maintains a predetermined relationship In a particular coordinate system 15 
between the force applied In one axial direction and the displacement in the same axial direction, and the 
robot controlling the arm end to move the tool to tracic a standard path obtained from prestored position 

data and to act on a workplace with a preset target force; the robot being characterized by further 
comprising: 

present-condition detection means for detecting a present position and attitude of the arm end; 20 
reaction-force detection means for detecting a reaction force transmitted from the workpiece to the tool 
at the detecting point of the present position and attitude of the arm end ; 

displacement conversion means for calculating the difference between the detected reaction force and 
the preset target force of the tool at the detecting point for the present position and attitude of the arm 
end, and for converting the difference into the displacement of the position of the tool; 25 
Ideal-condition calculation means for calculating an Ideal position and attitude of the arm end based on 
the displacement of the tool and on the present position and attitude of the arm end ; 
correction-amount calculation means for calculating a correction amount for the position and attitude of 
the arm end by comparing the Ideal position and attitude of the arm end with a reference position and 
attitude of the arm end on the standard path; and 30 
arm-end control means for controlling the position and attitude of the arm end in real time according to 
the correction amount for the position and attitude of the arm end. 

2. A tracking control robot according to claim 1 in which, 

the arm-end control means Include motors for driving the arm end, and 

the present-condition detection means include: a rotation sensor attached to the axes of the motors; and 35 
means for generating, in response to signals from the rotation sensor, position and attitude data for the 
end of the sensor and position and attitude data for the work point of the tool, the data being based on a 
predetermined point in the coordinate system, and for calculating the present position and attitude of the 
tool from these data. 

3. A tracking controi robot according to claim 1 in which the present-condition detection means and the 40 
reaction-force detection means simultaneously execute their respective detections. 

4. A tracking control robot according to claim 1 in which the reaction-force detection means are 
provided between the displacement-to-force conversion mechanism and the tool for detecting a value of 
the force on the displacement-to-force conversion mechanism, the detected value being the difference 
between the force of gravity on the tool and the reaction force transmitted from the workpiece to the tool. 45 

5. A tracking controi robot according to claim 1 in which the displacement conversion means Include 
means for calculating a degree of change In the reaction force according to a degree of change In the 
value offeree detected by the reaction-force detection means. 

6. A tracking controi robot according to claim 1 in which the displacement conversion means include 
means for calculating the displacement in the position of the tool such that the reaction force from the 50 
workpiece to the tool Is equal to the preset target force, by using an equation of motion of the tool as a 
material particle, the equation including the degree of change In the reaction force, and a viscosity 
coefficient and a spring constant for the dispiacement-to-force conversion mechanism. 

7. A tracking control robot according to claim 6 In which the Ideal-condition calculation means include 
means for calculating the Ideal position and attitude of the arm end according to the displacement In the 55 
position of the tool and the present position and attitude of the am end. 

8. A tracking control robot according to claim 1 in which: 

the robot comprises a storage means for holding data of reference position and attitude for the arm end 
on the standard path, the data being stored in advance according to a particular teaching method, and 
the con-ectlon-amount calculation means include means for reading out the reference position and 60 
attitude data from the storage means. 

9. A tracking control robot according to claim 1 In which the connection-amount calculation means 
include means for calculating a deviation between the ideal position and attitude of the arm end and the 
reference position and attitude of the arm end on the standard path, respectively, so as to calculate the 
correction amount for the position and attitude of the arm end. 65 
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10. A tracking control robot according to claim 1 In which the correction-amount calculation means 
include means for modifying the connection amount for the position and attitude of the arm end by a preset 
offset value. 

11. A tracking control robot according to claim 1 in which the correction-amount calculation means 
include means for comparing the con'ectlon amount for the position and attitude of the arm end with a 
predetermined value and for controlling In real time the position and attitude of the arm end only when the 
correction amount Is smaller than the predetermined value. 

12. A tracking control robot having a tool at an end of an arm of the robot with a dispiacement-to-force 
conversion mechanism that maintains a predetermined relationship in a sensor coordinate system 
between the force applied In one axial direction and the displacement In the same axial direction, and the 
robot controlling the arm end to move the tool to track a standard path obtained from prestored position 
data in a robot coordinate system and to act on a workpiece with a preset target force; the robot being 
characterized by further comprising : 

present-condition detection means for detecting a present position and attitude of the arm end in the 
sensor coordinate system ; 

reaction-force detection means for detecting a reaction force transmitted from the workpiece to the tool 
at the detecting point of the present position and attitude of the arm end; 

displacement conversion means for calculating the difference between the detected reaction force and 
the preset target force of the tool at the detecting point of the present position and attitude of the arm 
end. and for converting the difference into the displacement in the position of the tool; 
ideal-condition calculation means for calculating an Ideal position and attitude of the arm end in the 
sensor coordinate system based on the displacement of the tool and on the present position and attitude 
of the arm end; 

conversion means for converting the Ideal position and attitude of the arm end from the sensor 
coordinate system into the robot coordinate system; 

correction-amount calculation means for calculating a correction amount for the position and attitude of 
the ann end in the robot coordinate system by comparing the converted ideal position and attitude of the 
arm end with a reference position and attitude of the arm end on the standard path ; and 
arm-end control means for controlling the position and attitude of the arm end in real time according to 
the correction amount for the position and attitude of the arm end. 
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